﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.h
// Revision   : r29
// Change-Date: 2009-11-28
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/RevoluteJoint.cpp
// Revision   : r52
// Change-Date: 2010-02-02
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Revolute joint definition. This requires defining an
    /// anchor point where the bodies are joined. The definition
    /// uses local anchor points so that the initial configuration
    /// can violate the constraint slightly. You also need to
    /// specify the initial relative angle for joint limits. This
    /// helps when saving and loading a game.<br/><br/>
    /// The local anchor points are measured from the body's origin
    /// rather than the center of mass because:<br/>
    /// 1. you might not know where the center of mass will be.<br/>
    /// 2. if you add/remove shapes from a body and recompute the mass, the joints will be broken.</br>
    /// </summary>
    public class RevoluteJointDef : JointDef
    {
	    public RevoluteJointDef()
            : base()
	    {
		    type = JointType.REVOLUTE;
		    enableLimit = false;
		    enableMotor = false;
	    }

        /// <summary>Initialize the bodies, anchors, and reference angle using the world anchor.</summary>
        public void Initialize(Body body1, Body body2, Vector2 anchor)
        {
            this.body1 = body1;
            this.body2 = body2;
            localAnchor1 = body1.GetLocalPoint(anchor);
            localAnchor2 = body2.GetLocalPoint(anchor);
            referenceAngle = body2.Angle - body1.Angle;
        }

        /// <summary>The local anchor point relative to body1's origin.</summary>
        public Vector2 localAnchor1;

        /// <summary>The local anchor point relative to body2's origin.</summary>
	    public Vector2 localAnchor2;

        /// <summary>The body2 angle minus body1 angle in the reference state (radians).</summary>
        public float referenceAngle;

        /// <summary>A flag to enable joint limits.</summary>
        public bool enableLimit;

        /// <summary>The lower angle for the joint limit (radians).</summary>
        public float lowerAngle;

        /// <summary>The upper angle for the joint limit (radians).</summary>
        public float upperAngle;

        /// <summary>A flag to enable the joint motor.</summary>
        public bool enableMotor;

        /// <summary>The desired motor speed. Usually in radians per second.</summary>
        public float motorSpeed;

        /// <summary>The maximum motor torque used to achieve the desired motor speed.
	    /// Usually in N-m.</summary>
        public float maxMotorTorque;
    };

    /// <summary>A revolute joint constrains to bodies to share a common point while they
    /// are free to rotate about the point. The relative rotation about the shared
    /// point is the joint angle. You can limit the relative rotation with
    /// a joint limit that specifies a lower and upper angle. You can use a motor
    /// to drive the relative rotation about the shared point. A maximum motor torque
    /// is provided so that infinite forces are not generated.</summary>
    public class RevoluteJoint : Joint
    {
	    public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchor1);
        }

	    public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchor2);
        }

        /// <summary>
        /// Get the reaction force given the inverse time step. Unit is N.
        /// </summary>
	    public override Vector2 GetReactionForce(float inv_dt)
        {
	        Vector2 P = new Vector2(m_impulse.X, m_impulse.Y);
	        return inv_dt * P;
        }

        /// <summary>
        /// Get the reaction torque due to the joint limit given the inverse time step. Unit is N*m.
        /// </summary>
	    public override float GetReactionTorque(float inv_dt)
        {
            return inv_dt * m_impulse.Z;
        }

        /// <summary>Get the current joint angle in radians.</summary>
	    public float GetJointAngle()
        {
            Body b1 = m_body1;
            Body b2 = m_body2;
            return b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
        }

        /// <summary>Get the current joint angle speed in radians per second.</summary>
	    public float GetJointSpeed() 
        {
            Body b1 = m_body1;
            Body b2 = m_body2;
            return b2.m_angularVelocity - b1.m_angularVelocity;
        }

        /// <summary>Is the joint limit enabled?</summary>
	    public bool IsLimitEnabled() 
        {
            return m_enableLimit;
        }

        /// <summary>Enable/disable the joint limit.</summary>
        public void EnableLimit(bool flag)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_enableLimit = flag;
        }

        /// <summary>Get the lower joint limit in radians.</summary>
	    public float GetLowerLimit() 
        {
            return m_lowerAngle;
        }

        /// <summary>Get the upper joint limit in radians.</summary>
	    public float GetUpperLimit() 
        {
            return m_upperAngle;
        }

        /// <summary>Set the joint limits in radians.</summary>
        public void SetLimits(float lower, float upper)
        {
            System.Diagnostics.Debug.Assert(lower <= upper);
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_lowerAngle = lower;
            m_upperAngle = upper;
        }

        /// <summary>Is the joint motor enabled?</summary>
	    public bool IsMotorEnabled() 
        {
            return m_enableMotor;
        }

        /// <summary>Enable/disable the joint motor.</summary>
        public void EnableMotor(bool flag)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_enableMotor = flag;
        }

        /// <summary>Set the motor speed in radians per second.</summary>
        public void SetMotorSpeed(float speed)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_motorSpeed = speed;
        }

        /// <summary>Get the motor speed in radians per second.</summary>
	    public float GetMotorSpeed()
        {
            return m_motorSpeed;
        }

        /// <summary>Set the maximum motor torque, usually in N-m.</summary>
        public void SetMaxMotorTorque(float torque)
        {
            m_body1.Awake = true;
            m_body2.Awake = true;
            m_maxMotorTorque = torque;
        }

        /// <summary>Get the current motor torque given the inverse time step. Unit is N*m.</summary>
	    public float GetMotorTorque(float inv_dt) 
        {
            return inv_dt * m_motorImpulse;
        }

	    public RevoluteJoint(RevoluteJointDef def)
            : base(def)
        {
	        m_localAnchor1 = def.localAnchor1;
	        m_localAnchor2 = def.localAnchor2;
	        m_referenceAngle = def.referenceAngle;

	        m_impulse = Vector3.Zero;
	        m_motorImpulse = 0.0f;

	        m_lowerAngle = def.lowerAngle;
	        m_upperAngle = def.upperAngle;
	        m_maxMotorTorque = def.maxMotorTorque;
	        m_motorSpeed = def.motorSpeed;
	        m_enableLimit = def.enableLimit;
	        m_enableMotor = def.enableMotor;
	        m_limitState = LimitState.INACTIVE;
        }

        internal override void InitVelocityConstraints(TimeStep step)
        {
	        Body b1 = m_body1;
	        Body b2 = m_body2;

	        if (m_enableMotor || m_enableLimit)
	        {
		        // You cannot create a rotation limit between bodies that
		        // both have fixed rotation.
		        System.Diagnostics.Debug.Assert(b1.m_invI > 0.0f || b2.m_invI > 0.0f);
	        }

	        // Compute the effective mass matrix.
	        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
	        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

	        // J = [-I -r1_skew I r2_skew]
	        //     [ 0       -1 0       1]
	        // r_skew = [-ry; rx]

	        // Matlab
	        // K = [ m1+r1y^2*i1+m2+r2y^2*i2,  -r1y*i1*r1x-r2y*i2*r2x,          -r1y*i1-r2y*i2]
	        //     [  -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2,           r1x*i1+r2x*i2]
	        //     [          -r1y*i1-r2y*i2,           r1x*i1+r2x*i2,                   i1+i2]

	        float m1 = b1.m_invMass, m2 = b2.m_invMass;
	        float i1 = b1.m_invI, i2 = b2.m_invI;

	        m_mass.col1.X = m1 + m2 + r1.Y * r1.Y * i1 + r2.Y * r2.Y * i2;
	        m_mass.col2.X = -r1.Y * r1.X * i1 - r2.Y * r2.X * i2;
	        m_mass.col3.X = -r1.Y * i1 - r2.Y * i2;
	        m_mass.col1.Y = m_mass.col2.X;
	        m_mass.col2.Y = m1 + m2 + r1.X * r1.X * i1 + r2.X * r2.X * i2;
	        m_mass.col3.Y = r1.X * i1 + r2.X * i2;
	        m_mass.col1.Z = m_mass.col3.X;
	        m_mass.col2.Z = m_mass.col3.Y;
	        m_mass.col3.Z = i1 + i2;

            m_motorMass = i1 + i2;
            if (m_motorMass > 0.0f)
            {
                m_motorMass = 1.0f / m_motorMass;
            }

	        if (m_enableMotor == false)
	        {
		        m_motorImpulse = 0.0f;
	        }

	        if (m_enableLimit)
	        {
		        float jointAngle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
		        if (System.Math.Abs(m_upperAngle - m_lowerAngle) < 2.0f * PhysicSettings.AngularSlop)
		        {
			        m_limitState = LimitState.EQUAL;
		        }
		        else if (jointAngle <= m_lowerAngle)
		        {
			        if (m_limitState != LimitState.AT_LOWER)
			        {
				        m_impulse.Z = 0.0f;
			        }
			        m_limitState = LimitState.AT_LOWER;
		        }
		        else if (jointAngle >= m_upperAngle)
		        {
			        if (m_limitState != LimitState.AT_UPPER)
			        {
				        m_impulse.Z = 0.0f;
			        }
			        m_limitState = LimitState.AT_UPPER;
		        }
		        else
		        {
			        m_limitState = LimitState.INACTIVE;
			        m_impulse.Z = 0.0f;
		        }
	        }
	        else
	        {
		        m_limitState = LimitState.INACTIVE;
	        }

	        if (step.warmStarting)
	        {
		        // Scale impulses to support a variable time step.
		        m_impulse *= step.dtRatio;
		        m_motorImpulse *= step.dtRatio;

		        Vector2 P = new Vector2(m_impulse.X, m_impulse.Y);

		        b1.m_linearVelocity -= m1 * P;
		        b1.m_angularVelocity -= i1 * (Vector2Extensions.Cross(r1, P) + m_motorImpulse + m_impulse.Z);

		        b2.m_linearVelocity += m2 * P;
                b2.m_angularVelocity += i2 * (Vector2Extensions.Cross(r2, P) + m_motorImpulse + m_impulse.Z);
	        }
	        else
	        {
		        m_impulse = Vector3.Zero;
		        m_motorImpulse = 0.0f;
	        }
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
	        Body b1 = m_body1;
	        Body b2 = m_body2;

	        Vector2 v1 = b1.m_linearVelocity;
	        float w1 = b1.m_angularVelocity;
	        Vector2 v2 = b2.m_linearVelocity;
	        float w2 = b2.m_angularVelocity;

	        float m1 = b1.m_invMass, m2 = b2.m_invMass;
	        float i1 = b1.m_invI, i2 = b2.m_invI;

	        // Solve motor constraint.
	        if (m_enableMotor && m_limitState != LimitState.EQUAL)
	        {
		        float Cdot = w2 - w1 - m_motorSpeed;
		        float impulse = m_motorMass * (-Cdot);
		        float oldImpulse = m_motorImpulse;
		        float maxImpulse = step.dt * m_maxMotorTorque;
		        m_motorImpulse = MathHelper.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
		        impulse = m_motorImpulse - oldImpulse;

		        w1 -= i1 * impulse;
		        w2 += i2 * impulse;
	        }

	        // Solve limit constraint.
            if (m_enableLimit && m_limitState != LimitState.INACTIVE)
	        {
		        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
		        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

		        // Solve point-to-point constraint
		        Vector2 Cdot1 = v2 + Vector2Extensions.Cross(w2, r2) - v1 - Vector2Extensions.Cross(w1, r1);
		        float Cdot2 = w2 - w1;
		        Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2);

		        Vector3 impulse = m_mass.Solve33(-Cdot);

                if (m_limitState == LimitState.EQUAL)
		        {
			        m_impulse += impulse;
		        }
                else if (m_limitState == LimitState.AT_LOWER)
		        {
			        float newImpulse = m_impulse.Z + impulse.Z;
			        if (newImpulse < 0.0f)
			        {
				        Vector2 reduced = m_mass.Solve22(-Cdot1);
				        impulse.X = reduced.X;
				        impulse.Y = reduced.Y;
				        impulse.Z = -m_impulse.Z;
				        m_impulse.X += reduced.X;
				        m_impulse.Y += reduced.Y;
				        m_impulse.Z = 0.0f;
			        }
		        }
                else if (m_limitState == LimitState.AT_UPPER)
		        {
			        float newImpulse = m_impulse.Z + impulse.Z;
			        if (newImpulse > 0.0f)
			        {
				        Vector2 reduced = m_mass.Solve22(-Cdot1);
				        impulse.X = reduced.X;
				        impulse.Y = reduced.Y;
				        impulse.Z = -m_impulse.Z;
				        m_impulse.X += reduced.X;
				        m_impulse.Y += reduced.Y;
				        m_impulse.Z = 0.0f;
			        }
		        }

		        Vector2 P = new Vector2(impulse.X, impulse.Y);

		        v1 -= m1 * P;
		        w1 -= i1 * (Vector2Extensions.Cross(r1, P) + impulse.Z);

		        v2 += m2 * P;
		        w2 += i2 * (Vector2Extensions.Cross(r2, P) + impulse.Z);
	        }
	        else
	        {
		        Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
		        Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

		        // Solve point-to-point constraint
                Vector2 Cdot = v2 + Vector2Extensions.Cross(w2, r2) - v1 - Vector2Extensions.Cross(w1, r1);
		        Vector2 impulse = m_mass.Solve22(-Cdot);

		        m_impulse.X += impulse.X;
		        m_impulse.Y += impulse.Y;

		        v1 -= m1 * impulse;
                w1 -= i1 * Vector2Extensions.Cross(r1, impulse);

		        v2 += m2 * impulse;
                w2 += i2 * Vector2Extensions.Cross(r2, impulse);
	        }

	        b1.m_linearVelocity = v1;
	        b1.m_angularVelocity = w1;
	        b2.m_linearVelocity = v2;
	        b2.m_angularVelocity = w2;
        }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            // TODO_ERIN block solve with limit.

            //B2_NOT_USED(baumgarte);

            Body b1 = m_body1;
            Body b2 = m_body2;

            float angularError = 0.0f;
            float positionError = 0.0f;

            // Solve angular limit constraint.
            if (m_enableLimit && m_limitState != LimitState.INACTIVE)
            {
                float angle = b2.m_sweep.a - b1.m_sweep.a - m_referenceAngle;
                float limitImpulse = 0.0f;

                if (m_limitState == LimitState.EQUAL)
                {
                    // Prevent large angular corrections
                    float C = MathHelper.Clamp(angle - m_lowerAngle, -PhysicSettings.MaxAngularCorrection, PhysicSettings.MaxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                    angularError = System.Math.Abs(C);
                }
                else if (m_limitState == LimitState.AT_LOWER)
                {
                    float C = angle - m_lowerAngle;
                    angularError = -C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathHelper.Clamp(C + PhysicSettings.AngularSlop, -PhysicSettings.MaxAngularCorrection, 0.0f);
                    limitImpulse = -m_motorMass * C;
                }
                else if (m_limitState == LimitState.AT_UPPER)
                {
                    float C = angle - m_upperAngle;
                    angularError = C;

                    // Prevent large angular corrections and allow some slop.
                    C = MathHelper.Clamp(C - PhysicSettings.AngularSlop, 0.0f, PhysicSettings.MaxAngularCorrection);
                    limitImpulse = -m_motorMass * C;
                }

                b1.m_sweep.a -= b1.m_invI * limitImpulse;
                b2.m_sweep.a += b2.m_invI * limitImpulse;

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            // Solve point-to-point constraint.
            {
                Vector2 r1 = Math.Mul(b1.Transform.R, m_localAnchor1 - b1.LocalCenter);
                Vector2 r2 = Math.Mul(b2.Transform.R, m_localAnchor2 - b2.LocalCenter);

                Vector2 C = b2.m_sweep.c + r2 - b1.m_sweep.c - r1;
                positionError = C.Length();

                float invMass1 = b1.m_invMass, invMass2 = b2.m_invMass;
                float invI1 = b1.m_invI, invI2 = b2.m_invI;

                // Handle large detachment.
                float k_allowedStretch = 10.0f * PhysicSettings.LinearSlop;
                if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
                {
                    // Use a particle solution (no rotation).
                    Vector2 u = C; u.Normalize();
                    float m = invMass1 + invMass2;
                    if (m > 0.0f)
                    {
                        m = 1.0f / m;
                    }
                    Vector2 impulse = m * (-C);
                    const float k_beta = 0.5f;
                    b1.m_sweep.c -= k_beta * invMass1 * impulse;
                    b2.m_sweep.c += k_beta * invMass2 * impulse;

                    C = b2.m_sweep.c + r2 - b1.m_sweep.c - r1;
                }

                Mat22 K1 = new Mat22();
                K1.col1.X = invMass1 + invMass2;    K1.col2.X = 0.0f;
                K1.col1.Y = 0.0f;                   K1.col2.Y = invMass1 + invMass2;

                Mat22 K2 = new Mat22();
                K2.col1.X = invI1 * r1.Y * r1.Y;    K2.col2.X = -invI1 * r1.X * r1.Y;
                K2.col1.Y = -invI1 * r1.X * r1.Y;   K2.col2.Y = invI1 * r1.X * r1.X;

                Mat22 K3 = new Mat22();
                K3.col1.X = invI2 * r2.Y * r2.Y;    K3.col2.X = -invI2 * r2.X * r2.Y;
                K3.col1.Y = -invI2 * r2.X * r2.Y;   K3.col2.Y = invI2 * r2.X * r2.X; 

                Mat22 K = K1 + K2 + K3;
#if !OPTIMIZE
                Vector2 _impulse = K.Solve(-C);
#else
                Vector2 nC; nC.X = -C.X; nC.Y = -C.Y;
                Vector2 _impulse = K.Solve(ref nC.X, ref nC.Y);
#endif

                b1.m_sweep.c -= b1.m_invMass * _impulse;
                b1.m_sweep.a -= b1.m_invI * Vector2Extensions.Cross(r1, _impulse);

                b2.m_sweep.c += b2.m_invMass * _impulse;
                b2.m_sweep.a += b2.m_invI * Vector2Extensions.Cross(r2, _impulse);

                b1.SynchronizeTransform();
                b2.SynchronizeTransform();
            }

            return positionError <= PhysicSettings.LinearSlop && angularError <= PhysicSettings.AngularSlop;
        }

        public Vector2 m_localAnchor1;	// relative
        public Vector2 m_localAnchor2;
        public Vector3 m_impulse;
        public float m_motorImpulse;

        public Mat33 m_mass;		// effective mass for point-to-point constraint.
        public float m_motorMass;	// effective mass for motor/limit angular constraint.

        public bool m_enableMotor;
        public float m_maxMotorTorque;
        public float m_motorSpeed;

        public bool m_enableLimit;
        public float m_referenceAngle;
        public float m_lowerAngle;
        public float m_upperAngle;
        public LimitState m_limitState;
    };
}
